/**
 * Puny objects! You are nothing next to the SUPERMAN ALGORITHM!
 * 
 * This algorithm beelines for the goal while completely ignoring any obstacles on the map
 * A test algorithm
 */

using System;
using System.Collections.Generic;
using System.Text;
using System.Collections;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;

namespace Autobot
{
    class SupermanAlgorithm : AlgorithmBase
    {
        // Static string to identify this class in the Algorithms class
        public static readonly new string Name = "Superman";

        // Store a reference to the world objects
        public SupermanAlgorithm(VirtualMap mapReference, Robot robotReference, Goal goalReference)
        {
            map = mapReference;
            robot = robotReference;
            goal = goalReference;
        }

        // Degrees the goal can be off of the robot so that it will think of it as being in a straight line
        private const double StraightLineAngle = 1.00;

        /// <summary>
        /// The Superman Algorithm tell the robot to go straight to the goal ignoring all objects
        /// It calculates if it needs to turn left or right to reach the goal and sends steering motor controls accordingly
        /// </summary>
        /// <returns>List of commands to send to the robot</returns>
        public override AlgorithmResult GetNextResult()
        {
            // Get input to start and stop the robot
            KeyboardState input = Keyboard.GetState();

            AlgorithmResult NewResult = new AlgorithmResult();
            // Tell the robot to start moving when 'G' is pushed
            if (robot.GetVelocity() == 0.0f && input.IsKeyDown(Keys.G))
            {
                NewResult.AddCommand(CreateIncSpeedCommand(10, 0));
                previousResults.Push(NewResult);
                return previousResults.Peek();
            }

            // If we reached the goal then stop the car or if the user pushes 'S'
            if (goal.CheckGoalReached(robot.GetRealPosition()) || input.IsKeyDown(Keys.S))
            {
                NewResult.AddCommand(CreateStopCommand(0));
                previousResults.Push(NewResult);
                return previousResults.Peek();
            }

            /// Determine the angle the robot needs to go to reach the goal so the robot can turn left or right

            // Get the angle the robot is driving at, convert it to HUMAN terms,
            // Stupid XNA flipping the Y direction... argh
            Vector2 robotDir = robot.GetDirection();

            double robotAngle = Math.Atan2((double)robotDir.Y, (double)robotDir.X);

            // We want all the big angles
            // Robot is going up and to the right
            if ((robotDir.Y < 0) && (robotDir.X > 0))
                robotAngle = -robotAngle;
            // Robot is going up and to the left
            else if ((robotDir.Y < 0) && (robotDir.X < 0))
                robotAngle = -robotAngle;
            // Robot is going down and to the left
            else if ((robotDir.Y > 0) && (robotDir.X < 0))
                robotAngle = 2.0 * Math.PI - robotAngle;
            // Robot is goint down and to the right
            else if ((robotDir.Y > 0) && (robotDir.X > 0))
                robotAngle = 2.0 * Math.PI - robotAngle;

            Vector2 RobotPosition = robot.GetVirtualPosition();
            Vector2 GoalPosition = goal.GetVirtualPosition();

            Vector2 RobotToGoal = GoalPosition - RobotPosition;

            // Get the angle from the robot to the goal, convert it to HUMAN terms,
            // Stupid XNA flipping the Y direction... argh
            //RobotToGoal.Y = -RobotToGoal.Y;
            //RobotToGoal.X = -RobotToGoal.X;

            double RobotToGoalAngle = Math.Atan2((double)RobotToGoal.Y, (double)RobotToGoal.X);

            // If the goal is above and to the left of the robot
            if ((GoalPosition.Y < RobotPosition.Y) && (GoalPosition.X < RobotPosition.X))
            {
                RobotToGoalAngle = -RobotToGoalAngle;
            }
            // Goal above and to the right of the robot
            else if ((GoalPosition.Y < RobotPosition.Y) && (GoalPosition.X > RobotPosition.X))
            {
                RobotToGoalAngle = -RobotToGoalAngle;
            }
            // Goal below and to the left of robot
            else if ((GoalPosition.Y > RobotPosition.Y) && (GoalPosition.X < RobotPosition.X))
            {
                RobotToGoalAngle = 2 * Math.PI - RobotToGoalAngle;
            }
            // Goal below and to the right of robot
            else if ((GoalPosition.Y > RobotPosition.Y) && (GoalPosition.X > RobotPosition.X))
            {
                RobotToGoalAngle = 2 * Math.PI - RobotToGoalAngle;
            }

            // Head towards the goal!
            //NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
            if (robotAngle < RobotToGoalAngle)
            {
                if (robotAngle > (RobotToGoalAngle - Math.PI))
                    NewResult.AddCommand(CreateTurnLeftCommand((int)SteeringMotorParams.MAX_LEFT, 0));
                else
                    NewResult.AddCommand(CreateTurnRightCommand((int)SteeringMotorParams.MAX_RIGHT, 0));
            }
            else if (robotAngle > RobotToGoalAngle)
            {
                if (robotAngle < (RobotToGoalAngle + Math.PI))
                    NewResult.AddCommand(CreateTurnRightCommand((int)SteeringMotorParams.MAX_RIGHT, 0));
                else
                    NewResult.AddCommand(CreateTurnLeftCommand((int)SteeringMotorParams.MAX_LEFT, 0));
            }
            //NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 100));

            return NewResult;
        }
    }
}
